%第一题
clc;
clear all;
format long;

filename = 'mapclear.csv' ;
MapPoint=csvread(filename);   %三列分别为 点类型 X Y

[m,n]=size(MapPoint);

cmap=[0.75 0.75 0.75;  % 1 灰色 自由通行
    0    1    0; % 2 绿色 储位节点
    1    1    0;% 3 黄色 保留节点
    0    0    0;% 4 黑色 柱子节点
    0    0    1;% 5 蓝色 拣选工位节点
    1    0    1;% 6 粉色 补货位节点
    1    0    0;% 7 红色 空托盘回收节点
    ];
colormap(cmap);

Color=ones(23,33);  %pcolor从1开始画，这里行列+1
for i=1:1:m
    Color(MapPoint(i,3)+1,MapPoint(i,2)+1)= MapPoint(i,1);  %MapPoint从0开始计数，此处+1
end
for i=1:1:m
    MapPoint(i,3)=MapPoint(i,3)+1;
    MapPoint(i,2)=MapPoint(i,2)+1;
end
figure(1);
pcolor(Color);
hold on;
% 不同AGV所在PATH行数不同，更换下i即可画出所有图
load('PATH1.mat');
for i=1:1:5
    x=[];
    y=[];
    flag=1;
    for j=1:2:10000
        if PATH(i,j)~=0
            x(1,flag)=PATH(i,j)+0.5;
            y(1,flag)=PATH(i,j+1)+0.5;
            flag=flag+1;
        end
        if PATH(i,j)==0
            
            a(i,1)= plot(x,y,'linewidth',5);
            break;
        end
    end
end

legend([a(1,1),a(2,1),a(3,1),a(4,1),a(5,1)],'Car 1','Car 2','Car 3','Car 4','Car 5');
% legend([a(1,1),a(2,1),a(3,1),a(4,1),a(5,1)],'Car 6','Car 7','Car 8','Car 9','Car 10');
% legend([a(1,1),a(2,1),a(3,1),a(4,1),a(5,1)],'Car 11','Car 12','Car 13','Car 14','Car 15');
% legend([a(1,1),a(2,1),a(3,1),a(4,1)],'Car 16','Car 17','Car 18','Car 19');
set(gca,'xtick',[],'xticklabel',[]);
set(gca,'ytick',[],'yticklabel',[]);









